/*
 *  TrackLearner.cpp
 *  powaah
 *
 *  Created by Tim Uusitalo on 4/26/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "TrackLearner.h"

#include "MathUtility.h"

#include "FileHandler.h"

using namespace MathUtility;

TrackLearner::TrackLearner()
: nr_corners(0), current_corner(0), offtrack(0), last_corner(0)
{

	off_road_lock = false;
	
}

TrackLearner::~TrackLearner()
{
	
}

void TrackLearner::update(CarState& cs)
{
	if (track.size() > 2)
	{
		int closest = 0;
		float diffDist = 30000.0f;
		
		for (unsigned int i = 0; i < track.size(); i++)
		{
			if (track[i].dist_raced - cs.getDistFromStart() < diffDist && track[i].dist_raced - cs.getDistFromStart() > 0)
			{
				closest = i;
				diffDist = track[i].dist_raced - cs.getDistFromStart();
			}
		}
		last_corner = current_corner;
		current =  track[closest];
		current_nr = closest;
	}
	
	
	
	if (track.size() > 2)
	{
		unsigned int i = current_nr;
		while (i < track.size() && track[i].track_type == TRACK_TYPE_NONE)
		{
			i++;
			if (i == track.size())
			{
				i = 0;
			}
		}
		
		closest_turn = track[i];
		closest_nr = i;
	}
	 
	
}

void TrackLearner::record(CarState& cs, float width, float* const angles, const float speed, CarControl cc)
{
	TrackNode trk;
	int pos = 0;


	//std::cout << cs.getSpeedX() / fabs(cs.getSpeedZ()) << std::endl;
	for (int i = 1; i < 19; i++)
	{
		if (cs.getTrack(i) > cs.getTrack(pos))
		{
			pos = i;
		}
		
	}
	float s = cs.getTrack(9);
	s += cs.getTrack(7);
		s += cs.getTrack(8);
		s += cs.getTrack(10);
		s += cs.getTrack(11);
	s /= 5.0f;
	
	//trk.angle = (2 * cs.getAngle() - (cs.getTrackPos()*0.5f));
	if (track.size() > 1)
		trk.length = cs.getDistFromStart() - track.back().dist_raced;
	else 
		trk.length = cs.getDistFromStart();
	
	
	if (cs.getTrack(9) >= 70.0f /*|| cs.getTrack(9) >= cs.getTrack(8) && cs.getTrack(9) >= cs.getTrack(10)*/)
	{
		trk.angle = 0.0f;
		
	}
	else if (cs.getTrack(8) > cs.getTrack(10))
	{
		trk.track_type = TRACK_TYPE_LEFT;
		Vector2 pos1, pos2;
		pos1 = calculatePosition(convertDegToRad(angles[8]), cs.getTrack(8));
		pos2 = calculatePosition(convertDegToRad(angles[10]), cs.getTrack(10));
		
		trk.k = (pos2.y - pos1.y) / (pos2.x - pos1.x);
		
					
			trk.angle = cs.getAngle();
		
	}
	else if (cs.getTrack(8) < cs.getTrack(10))
	{
		trk.track_type = TRACK_TYPE_RIGHT;
		
		Vector2 pos1, pos2;
		pos1 = calculatePosition(convertDegToRad(angles[8]), cs.getTrack(8));
		pos2 = calculatePosition(convertDegToRad(angles[10]), cs.getTrack(10));
		
		
			TrackNode p0 = track[track.size()-2], p1 = track[track.size()-1], p2 = trk;
			trk.angle = cs.getAngle();
		
		
	}
	
	trk.dist_raced = cs.getDistFromStart();
	
	
	float left = 0, right = 0;
	
	if (cs.getTrack(18) <= 0)
		right = 0;
	else
		right = (cosf(cs.getAngle()) * cs.getTrack(18));
	if (cs.getTrack(0) <= 0)
	{
		left = 0;
	}
	else 
		left = (cosf(-cs.getAngle()) * cs.getTrack(0));
	trk.width = left + right;
	
	
	track.push_back(trk);
	
	if (trk.track_type == TRACK_TYPE_NONE)
	{
		std::cout << "None, angle: " << trk.angle << " width: " <<  trk.width << " dist raced: " << trk.dist_raced <<" k: " << trk.k << std::endl;

	}
	
	else if (trk.track_type == TRACK_TYPE_LEFT)
	{
		std::cout << "Left, angle: " << trk.angle << " width: " <<  trk.width << " dist raced: " << trk.dist_raced <<" k: " << trk.k << std::endl;

	}
	else if (trk.track_type == TRACK_TYPE_RIGHT)
	{
		std::cout << "Right, angle: " << trk.angle << " width: " <<  trk.width << " dist raced: " << trk.dist_raced <<" k: " << trk.k  << std::endl;

	}
	
}

TrackNode TrackLearner::getClosestNode(const float dist)
{
	return current;
}

const float TrackLearner::getDistToNextTurn(const float current, TrackNode* const node)
{
	if (track.size() > 0)
	{
		if (node)
			*node = track[closest_nr]; 
		return closest_turn.dist_raced - current;
	}
	
	return 0.0f;
	
	
}

void TrackLearner::save()
{
	
	FileDesc desc;
	desc.file_name = "track.trk";
	
	std::fstream file(desc.file_name.c_str(), std::ios::out | std::ios::binary);
	
	// if file exists
	if (file.is_open())
	{
		unsigned int size = track.size();
		
		desc.size = sizeof(unsigned int);
		
		desc.data = (char*)&size;
		desc.elements = 1;
		
		// save header first
		FileHandler::writePartBinary(file, desc);
		
		
		desc.size = sizeof(TrackNode);
		desc.data = (char*)&track[0];
		desc.elements = size;
		
		// save track data
		FileHandler::writePartBinary(file, desc);
		
		evaluateCorners();
	}

}

bool TrackLearner::load()
{
	FileDesc desc;
	desc.file_name = "track.trk";
	
	std::fstream file(desc.file_name.c_str(), std::ios::in | std::ios::binary);
	
	// if file exists
	if (file.is_open())
	{
		if (FileHandler::getBinaryFileSize(desc.file_name) == 0)
			return false;
		unsigned int data1 = 0;
		desc.size = sizeof(unsigned int);
		
		desc.data = (char*)&data1;
		
		desc.elements = 1;
	
		FileHandler::readPartBinary(file, desc);
		if (data1 > 0)
		{
			// now when we got how many items we shall save, make space for it
			track.resize(data1);
			
			desc.size = sizeof(TrackNode);
			desc.elements = data1;
			desc.data = (char*)&track[0];
			FileHandler::readPartBinary(file, desc);
			
			evaluateCorners();
			
			return true;
		}
		else 
		{
			return false;
		}

		return true;
	}
	
	return false;
}

const float TrackLearner::calculateRadius(const float p0, const float a0)
{
	float height = p0;//*sin(a0);
	
	float width = a0;//sqrt((p0*p0)-(height*height));
	
	float radius = width * width;
	radius /= (8*height);
	radius += ((2*height)/2);
	return radius;
}

const float TrackLearner::calculateRadius(const float p0, const float p1, const float a0, const float a1)
{
	float height = p0 * sin(a0);
	float width = (p1 * sin(pi - a1) / sin(a0));
	
	float radius = width * width;
	radius /= (8*height);
	radius += ((2*height)/2);
	return radius;
}

const float TrackLearner::calculateRadiusForTurn(const unsigned int start)
{
	TrackNode start_node = track[start];
	unsigned int i = start;
	
	while (i < track.size() && track[i].track_type == start_node.track_type)
	{
		
	}
	return 0;
}


const int TrackLearner::getCornerNr()
{
	return 0;
}

void TrackLearner::evaluateCorners()
{
	
	for (int i = 0; i < nr_corners; i++)
	{
		corner_offsets.push_back(1);
	}
	
	for (int i = 0; i < track.size(); i++)
	{
		if (track[i].track_type == TRACK_TYPE_NONE)
			std::cout << "straight" << std::endl;
		else if (track[i].track_type == TRACK_TYPE_LEFT)
			std::cout << "left" << std::endl;
		else if (track[i].track_type == TRACK_TYPE_RIGHT)
			std::cout << "right" << std::endl;
		
	}
	float width = 0.0f;
	float divide_width_by = 0;
	
	for (unsigned int i = 0; i < track.size(); i++)
	{
		width += track[i].width;
		divide_width_by++;
	}
	
	width /= divide_width_by;
	
	this->width = width;
	std::cout << "corners: " << nr_corners << "width: " << width << std::endl;
}

const float TrackLearner::getCornerOffset(const unsigned int i)
{
	
	return corner_offsets[i];
}


const float TrackLearner::getWidth() const
{
	return width;
}